#ifndef _AIM_AUTO_HPP
#define _AIM_AUTO_HPP 
#include <ros/ros.h>
#include <thread>
#include <deque>
#include <vector>
#include <iostream>
#include <numeric>
#include "ArmorPlate.hpp"
#include "LightMatch.hpp"
#include "aim_deps/AimDeps.hpp"
#include "aim_rec/NumRec.hpp"
#include "aim_pos/GetPos.hpp"
#include "aim_pre/Predict.hpp"
#include "aim_dec/Armordecision.hpp"
#include "aim_spin/AntiSpin.hpp"
#include "aim_tower/HitTower.hpp"
#include "serial_com/comm.h"
#include "readParam.hpp"

//===========远距离攻击类================
class AimAuto {
public:
    AimAuto(ros::NodeHandle& nh);                   //默认敌人为蓝色
    ~AimAuto();
public:
    /**
     * @brief 远距离打击一条龙集成处理
     * @param src 由取流解码得到的图像，必要的时候还会在图像上进行绘制
     * @param msg 来自云台的信息(并写入作为输出)
    */
    void aimAuto(cv::Mat &src, serial_com::comm &msg);       //一条龙服务

    void reset();                                       // reset预测器

    void setPredictState(bool stat) {
        this->use_prediction = stat;
        if (stat == false && aimp.isInit() == true) {   // 关闭需要重置
            aimp.reset();
        }
    }

    void enableHitTower(bool stat) {
        this->hit_tower = stat;
    }
    
    void overwriteColor(const bool reverse) {
        match.overwriteColor(reverse);
    }
private:
    //===================自瞄内部基本模式函数====================//
    void TargetDecisionByCenter(serial_com::comm &msg);      //处理操作手通过图传中心指定车状态下的自瞄决策
    bool findOptimalCar();
    void PickTarget();
    void CalcuCenter();
    //=======================================================//
    void aimRecognition(const cv::Mat &src);                 //对所有待选装甲板进行数字识别
    void hitSpinTower(serial_com::comm &msg);
    void aimDisplay(cv::Mat &src, const serial_com::comm &msg);//信息显示在屏幕上（debug功能）  

    /// TODO:需要删除的debug输出
    void aimDebugDisplay(cv::Mat &src, const serial_com::comm &msg);
    /**
     * @brief 计算弹道模型：now_delay在这个地方取得，同时也在此处判断是否发弹
     * @param msg 把弹道相关的角度写入msg中
     * @param pos 指定对pos位的tar_list进行计算,如果为NULLPOS，则对成员变量(int)optimal位解算
     * @note idle模式以及seqmatch模式不发弹在此处判断,预测精准性对发弹的影响判断也在此处
     */
    void aimBallistic(serial_com::comm &msg);       //弹道模型解算（只解算最优装甲板）
private:
    ArmorPlate amp;
    LightMatch match;
    Predict aimp;
    PnPdecision aim_dec; 
    GetPos pos_getter;
    AntiSpin anti_spin;
    HitTower tower_hitter;
    NumRec classifier;                                          //数字分类器
    Eigen::Vector3d camera_p;

    //===================//
    int optimal;                                            // 最优装甲板的下标（tar_list中的位置）
    int last_match;                                       // 上次匹配数字
    int optimal_carNumber = -1;
    uint8_t no_target_cnt;
    bool _is_enemy_blue;
    float _cut_ratio;
    bool use_prediction;
    bool hit_tower;
    bool use_antispin;
    bool is_spin;
    float spin_center;                                      //陀螺旋转中心
    std::vector<aim_deps::Armor> tar_list;                  // 多模块共用的装甲板预选容器
    std::deque<int> history_tar;
    Eigen::Vector3d Vision_p;                //图传相对于相机坐标系的平移向量
    Eigen::Matrix3d K_vision;
    Eigen::Matrix<double, 3, 4> K_camera;
    Eigen::Matrix3d Rinv;                   // Rinv是R的逆，而R与t可以组成原来的Vision_RT
    Eigen::Vector3d t;
    cv::Point2f CameraCenter; 

#ifdef DISPLAY_TARGETS
    char key;
    int _delay;                 //按下e键之后，会以_delay ms每帧的方式进行播放(并修改slow_judge)
    bool slow_judge;            //为true时，会以600ms每帧的方式播放
#endif //DISPLAY_TARGETS
};
#endif //_AIM_AUTO_HPP